#!/usr/bin/env python
# Author: iris and jeff

PKG = 'sound_localization'
NAME = 'robot_location_detector'

import roslib; roslib.load_manifest(PKG)
import rospy


from geometry_msgs.msg import * # the package has a Point msg
from nav_msgs.msg import Odometry

class RobotLocationDetector():
    def __init__(self):
        rospy.init_node(NAME, anonymous=True)
        rospy.Subscriber("/erratic_odometry/odom", Odometry, self.callback)
        self.pub = rospy.Publisher('/sound_localization/robot_location', Point)
        

    def callback(self,data): 
        try:
            #received data and store x,y,z as point
            point = Point()
            point.x = data.pose.pose.position.x
            point.y = data.pose.pose.position.y
            point.z = data.pose.pose.position.z
            #publish the point 
            self.pub.publish(point)
            #print "robot location just received %s" , point
        except rospy.ServiceException, e:
            print "Reply failed: %s"%e



if __name__ == '__main__':
    try:
        st = RobotLocationDetector()
        rospy.spin()
    except rospy.ROSInterruptException:
        pass
